using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using surfInterop;
using DotNetMatrix;

namespace Blimpbots.Recognition
{
    public struct LikelyObject
    {
        private Point2D _center;
        private double _depth;
        public WarpModel WarpModel;
        //public AffineModel AffineModel;
        public RecognitionExample Example;
        public RecognitionModel Model;

        public LikelyObject(RecognitionExample example, RecognitionModel model, WarpModel warpModel)
        {
            Model = model; 
            WarpModel = warpModel;
            Example = example;
            _depth = 0; _center = new Point2D();
            ComputeCenter();
        }

        public RectangleF RubberBand()
        {
            float Left = float.MaxValue, Right = 0f,
            Top = float.MaxValue, Bottom = 0f;

            // Rubber-band the points with a rectangle
            foreach (PointF pt in Example.ProjectSquare(WarpModel))
            {
                if (pt.X < Left) Left = pt.X;
                if (pt.X > Right) Right = pt.X;
                if (pt.Y < Top) Top = pt.Y;
                if (pt.Y > Bottom) Bottom = pt.Y;
            }

            return new RectangleF(Left, Top, Right - Left, Bottom - Top);
        }

        /// <summary>
        /// Filters keypoints using a rectangular rubber-band around the projections
        /// of the keypoints present in the image.
        /// </summary>
        /// <param name="all"></param>
        /// <returns></returns>
        public SurfKeypoint[] FilterKeypoints(SurfKeypoint[] all)
        {
            RectangleF region = RubberBand();
            region.Inflate(10F, 10F);

            List<SurfKeypoint> inside = new List<SurfKeypoint>(all.Length);
            foreach (SurfKeypoint kpt in all)
            {
                if (region.Contains(new PointF((float)kpt.x, (float)kpt.y)))
                    inside.Add(kpt);
            }
            return inside.ToArray();
        }

        //public SurfKeypoint[] FilterKeypoints(bool inside, IList<SurfKeypoint> keypoints)
        //{
        //    //TODO: use actual outline to do a better job

        //    List<SurfKeypoint> desiredKeypoints = new List<SurfKeypoint>();

        //    // Find the mapping from data space back to model space
        //    RectangleF modelRegion = Example.BoundingBox;
        //    modelRegion.Offset(-modelRegion.Left, -modelRegion.Top);
        //    GeneralMatrix inverseAffine = AffineModel.H.Inverse();

        //    foreach (SurfKeypoint k in keypoints)
        //    {
        //        GeneralMatrix kMat = new GeneralMatrix(new double[] { k.x, k.y, 1.0 }, 3);

        //        GeneralMatrix mdlCoord = inverseAffine.Multiply(kMat);
        //        double norm = mdlCoord.Array[2][0];
        //        PointF mdlPt = new PointF((float)(mdlCoord.Array[0][0] / norm),
        //                                  (float)(mdlCoord.Array[1][0] / norm));

        //        if (modelRegion.Contains(mdlPt))
        //            if(inside) desiredKeypoints.Add(k);
        //        else if (!inside)
        //            desiredKeypoints.Add(k);
        //    }

        //    return desiredKeypoints.ToArray();
        //}

        public double ObjectRadius {
            get {
 
                // ToDo: find the real front to center distance
                //       in the original image

                // pseudocode;  dist( front 3D point - center 3D point )
                //              dist offset from center by [1 0 0] vector
                //              projected back to 2D plane and dist
                //                 extracted.

                return 100;            
            }
        }

        public EstimatedPoint[] EstimatePoints(ICamera c)
        {
            EstimatedPoint[] est = EstimatePoints2D();
            return EstimatePoints3D(est, c, 1.0);
        }

        public EstimatedPoint[] EstimatePoints2D()
        {
            System.Drawing.RectangleF bound = Example.BoundingBox;
            List<EstimatedPoint> pts = new List<EstimatedPoint>(Example.NamedPoints.Count);
            foreach (KeyValuePair<string, NamedPoint> kvp in Example.NamedPoints)
            {
                double x = kvp.Value.Point.X - bound.Left;
                double y = kvp.Value.Point.Y - bound.Top;
                Point2D pt2Dd = WarpModel.PredictPoint(new Point2D(x, y));
                pts.Add(new EstimatedPoint(kvp.Key, default(Point3D), pt2Dd, 0, 0));
            }
            return pts.ToArray();
        }

        // depthRatio may be modified so that the center is projected to the right place
        public EstimatedPoint[] EstimatePoints3D(EstimatedPoint[] pts2D, ICamera c, double depthRatio)
        {
            for (int i = 0; i < pts2D.Length; i++)
            {
                Point2D pt2Dd = pts2D[i].ImagePosition;
                Point2D pt2Du = c.Undistort(pt2Dd);

                double distMeters = Example.NamedPoints[pts2D[i].Name].Distance;
                double d = depthRatio * ((distMeters * 1000) / WarpModel.Scale);

                GeneralMatrix ellipseRot;

                Point3D pt3Dd = c.EstimateCoordinate(pt2Dd, d, out ellipseRot);
                Point3D pt3Du = c.EstimateCoordinate(pt2Du, d, out ellipseRot);

                // Distance between distorted and undistorted projections
                double dist = pt3Dd.Distance(pt3Du);

                // Estimate true point as the middle of the distorted & undistorted pts
                Point3D est = new Point3D((pt3Dd.X + pt3Du.X) / 2,
                                          (pt3Dd.Y + pt3Du.Y) / 2,
                                          (pt3Dd.Z + pt3Du.Z) / 2);

                pts2D[i].EllipseRotation = ellipseRot.RowPackedCopy; /* error ellipse rotation */
                pts2D[i].Confidence = dist;
                pts2D[i].Position = est;
                pts2D[i].Depth = d;
            }

            return pts2D;
        }

        public Point2D Center { get { return _center; } }
        public double DepthEstimate { get { return _depth; } }

        private void ComputeCenter()
        {
            System.Drawing.RectangleF bound = Example.BoundingBox;

            double centerX = Example.Width / 2;
            double centerY = Example.Height / 2;
            double depth = 1000 / WarpModel.Scale; // distance in mm

            if (Example.NamedPoints.ContainsKey("center"))
            {
                NamedPoint centerNpt
                    = Example.NamedPoints["center"];

                System.Drawing.Point namedCenter = centerNpt.Point;
                centerX = (double)namedCenter.X - bound.Left;
                centerY = (double)namedCenter.Y - bound.Top;
                depth = (centerNpt.Distance * 1000) / WarpModel.Scale; // mm distance
            }

            //Point2D center = Geometry.ProjectPoint(centerX, centerY, AffineModel.H);

            Point2D center = WarpModel.PredictPoint(new Point2D(centerX, centerY));
            _center = center;
            _depth = depth;
        }

    }


}
